/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.28
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *  @file palletcalculate.cpp
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#include "palletcalculate.h"
#include <Eigen/Dense>
#include <QDebug>
#include <QObject>
#include <QString>
#include <QJsonDocument>
#include <QJsonArray>
#include <QFile>
#include <QJsonObject>
#include "messagelog.h"
using namespace Eigen;

#define IF_DEBUG false

PalletCalculate::PalletCalculate(int robotIdIn, QString configFilePathIn)
{
    robotId=robotIdIn;
    configFilePath=configFilePathIn;


    if(0)
    {
        palletMiddleInfoList.resize(10);
        writePalletInfo(configFilePathIn);
    }

    QString readComent;
    int tmpReturn = readConfigFile(configFilePathIn,readComent);
    if(1!=tmpReturn)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "WeldStartInstructionLogic",  12850,
                robotId ,0,0,0,0,readComent);
        palletMiddleInfoList.resize(10);
        if(0)
        {

            writePalletInfo(configFilePathIn);
        }
        else
        {
            for(int i=0;i<palletMiddleInfoList.size();i++)
            {
                palletMiddleInfoList[i].configInfo.stackNumber=i+1;
            }
        }
    }
}

PalletCalculate::~PalletCalculate()
{

}

int PalletCalculate::getPalletConditionInfo( std::vector<PalletMiddleInfo> &infoOut)
{
    usingMutex.lock();
    infoOut=palletMiddleInfoList;
    usingMutex.unlock();
    return 1;
}

int PalletCalculate::setPalletConditionInfo(StackInfo infoIn)
{
    usingMutex.lock();
    for(int i=0;i<palletMiddleInfoList.size();i++)
    {
        if(palletMiddleInfoList[i].configInfo.stackNumber==infoIn.stackNumber)
        {
            palletMiddleInfoList[i].configInfo=infoIn;
            usingMutex.unlock();
            return writePalletInfo(configFilePath);
        }
    }
    usingMutex.unlock();
    return 0;
}

PalletMiddleInfo PalletCalculate::getPalletInfoByIndex(int palletIndex)
{
    PalletMiddleInfo tmpMiddleInfo;
    if( palletIndex >= palletMiddleInfoList.size() || 0 > palletIndex)
    {
        return tmpMiddleInfo;
    }
    tmpMiddleInfo=palletMiddleInfoList[palletIndex];
    return tmpMiddleInfo;
}

int PalletCalculate::palletCalculateOnce(int palletIndex, int &row, int &col, int &layer)
{


    int tmpKey=calculatePalletPointByRcl(palletIndex,row,col,layer);
    if(1!=tmpKey)
    {
        return tmpKey;
    }
    tmpKey=calculateNextRcl(palletIndex,row,col,layer);
    if(1!=tmpKey)
    {
        addMsg(ENUM_MSG_REMIND, "PalletCalculate", "PalletCalculate", 12858,robotId, row,col,layer);
        row=0;
        col=0;
        layer=0;
        return 1;
    }
    return 1;
}



int PalletCalculate::calculatePalletPointByRcl(int palletIndex, int row, int col, int layer)
{
    if( palletIndex >= palletMiddleInfoList.size() || 0 > palletIndex)
    {
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12862,robotId, palletIndex+1);
        return -1;
    }
    usingMutex.lock();
    int tmpKey=0;
    switch (palletMiddleInfoList[palletIndex].configInfo.stackType)
    {
    case E_STACK_TYPE_B:
    case E_STACK_TYPE_BX:
    case E_STACK_TYPE_E:
    case E_STACK_TYPE_EX:
        tmpKey=calculatePalletPointByRcl_B(palletMiddleInfoList[palletIndex],row,col,layer);
        break;
    case E_STACK_TYPE_F:
    tmpKey=calculatePalletPointByRcl_F(palletMiddleInfoList[palletIndex],row,col,layer);
    break;
    default:
        tmpKey=0;
        break;
    }

    usingMutex.unlock();
    return tmpKey;

}

int calculateDeltaDistanceByTeach(const PalletMiddleInfo &tmpMiddleInfo,Vector3f &deltaRowVector,
                                  Vector3f &deltaColVector,Vector3f &deltaLayerVector)
{
    if(4>tmpMiddleInfo.configInfo.stackStylePointInfo.size())
    {
        qDebug()<<"error,4>tmpMiddleInfo.configInfo.stackStylePointInfo.size()";
        return 0;
    }
    if(6>tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue.size())
    {
        qDebug()<<"error,6>tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue.size()";
        return 0;
    }
    if(6>tmpMiddleInfo.configInfo.stackStylePointInfo[1].stackPoint.positionValue.size())
    {
        qDebug()<<"error,6>tmpMiddleInfo.configInfo.stackStylePointInfo[1].stackPoint.positionValue.size()";
        return 0;
    }
    if(6>tmpMiddleInfo.configInfo.stackStylePointInfo[2].stackPoint.positionValue.size())
    {
        qDebug()<<"error,6>tmpMiddleInfo.configInfo.stackStylePointInfo[2].stackPoint.positionValue.size()";
        return 0;
    }
    if(6>tmpMiddleInfo.configInfo.stackStylePointInfo[3].stackPoint.positionValue.size())
    {
        qDebug()<<"error,6>tmpMiddleInfo.configInfo.stackStylePointInfo[3].stackPoint.positionValue.size()";
        return 0;
    }
    //暂时不支持设置间距
    Vector3f startV(tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue[0],
            tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue[1],
            tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue[2]);
    Vector3f endRowV(tmpMiddleInfo.configInfo.stackStylePointInfo[1].stackPoint.positionValue[0],
            tmpMiddleInfo.configInfo.stackStylePointInfo[1].stackPoint.positionValue[1],
            tmpMiddleInfo.configInfo.stackStylePointInfo[1].stackPoint.positionValue[2]);
    Vector3f endColV(tmpMiddleInfo.configInfo.stackStylePointInfo[2].stackPoint.positionValue[0],
            tmpMiddleInfo.configInfo.stackStylePointInfo[2].stackPoint.positionValue[1],
            tmpMiddleInfo.configInfo.stackStylePointInfo[2].stackPoint.positionValue[2]);
    Vector3f endLayerV(tmpMiddleInfo.configInfo.stackStylePointInfo[3].stackPoint.positionValue[0],
            tmpMiddleInfo.configInfo.stackStylePointInfo[3].stackPoint.positionValue[1],
            tmpMiddleInfo.configInfo.stackStylePointInfo[3].stackPoint.positionValue[2]);
    if(1>=tmpMiddleInfo.configInfo.row)
    {
        deltaRowVector=endRowV-endRowV;
    }
    else
    {
        deltaRowVector=(endRowV-startV)/(tmpMiddleInfo.configInfo.row-1);
    }

    if(1>=tmpMiddleInfo.configInfo.col)
    {
        deltaColVector=endRowV-endRowV;
    }
    else
    {
        deltaColVector=(endColV-startV)/(tmpMiddleInfo.configInfo.col-1);
    }

    if(1>=tmpMiddleInfo.configInfo.layer)
    {
        deltaLayerVector=endRowV-endRowV;
    }
    else
    {
        deltaLayerVector=(endLayerV-startV)/(tmpMiddleInfo.configInfo.layer-1);
    }



    return 1;

}


int PalletCalculate::calculatePalletPointByRcl_B(PalletMiddleInfo &tmpMiddleInfo, int row,
                                                 int col, int layer)
{
    if(( row > tmpMiddleInfo.configInfo.row )||(1 > row) ||
        (col > tmpMiddleInfo.configInfo.col)||(1 > col) ||
        (layer > tmpMiddleInfo.configInfo.layer) ||(1 > layer))
    {//行列层超出范围
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12855,robotId, row,col,layer);
        return -20;
    }

    if(6>tmpMiddleInfo.configInfo.stackPointBTM.positionValue.size())
    {
        qDebug()<<"error,6>tmpMiddleInfo.stackPointBTM.positionValue.size()";
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12853,robotId, 1);
        return 0;
    }

    if(1>tmpMiddleInfo.configInfo.stackPointR_nList.size() )
    {
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12863,robotId, 1);
        return 0;
    }

    if(1>tmpMiddleInfo.configInfo.stackPointA_nList.size())
    {
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12864,robotId, 1);
        return 0;
    }

    Vector3f deltaRowVector,deltaColVector,deltaLayerVector;
    int tmpKey=calculateDeltaDistanceByTeach(tmpMiddleInfo,deltaRowVector,deltaColVector,deltaLayerVector);
    if(1!=tmpKey)
    {
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12857,robotId, 1);
        return tmpKey;
    }

    tmpMiddleInfo.btmPointPro=tmpMiddleInfo.configInfo.stackPointBTM;
    Vector3f tmpMove_row = (row-1) * deltaRowVector;
    Vector3f tmpMove_col = (col-1) * deltaColVector;
    Vector3f tmpMove_layer = (layer-1) * deltaLayerVector;
    Vector3f tmpMove_total=tmpMove_row+tmpMove_col+tmpMove_layer;

    tmpMiddleInfo.btmPointPro.positionValue[0] += tmpMove_total[0];
    tmpMiddleInfo.btmPointPro.positionValue[1] += tmpMove_total[1];
    tmpMiddleInfo.btmPointPro.positionValue[2] += tmpMove_total[2];

    tmpMiddleInfo.pointAnPointPro=tmpMiddleInfo.configInfo.stackPointA_nList[0].points;
    tmpMiddleInfo.pointRnPointPro=tmpMiddleInfo.configInfo.stackPointR_nList[0].points;
    for(int i=0; i<tmpMiddleInfo.configInfo.stackProximityCount; i++)
    {
        if(6>tmpMiddleInfo.pointAnPointPro[i].positionValue.size())
        {
            qDebug()<<"error,6>tmpMiddleInfo.pointAnPointPro[i].positionValue.size()"<<i;
            addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12851,robotId, i+1);
            return 0;
        }
        tmpMiddleInfo.pointAnPointPro[i].positionValue[0] += tmpMove_total[0];
        tmpMiddleInfo.pointAnPointPro[i].positionValue[1] += tmpMove_total[1];
        tmpMiddleInfo.pointAnPointPro[i].positionValue[2] += tmpMove_total[2];
    }
    for(int i=0; i<tmpMiddleInfo.configInfo.stackFleeCount; i++)
    {
        if(6>tmpMiddleInfo.pointRnPointPro[i].positionValue.size())
        {
            qDebug()<<"error,6>tmpMiddleInfo.pointRnPointPro[i].positionValue.size()"<<i;
            addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12852,robotId, i+1);
            return 0;
        }
        tmpMiddleInfo.pointRnPointPro[i].positionValue[0] += tmpMove_total[0];
        tmpMiddleInfo.pointRnPointPro[i].positionValue[1] += tmpMove_total[1];
        tmpMiddleInfo.pointRnPointPro[i].positionValue[2] += tmpMove_total[2];
    }
    return 1;
}

int calculateMovedVector(PointPro dest,PointPro origin,Vector3f &movedVector)
{
    Vector3f destV(dest.positionValue[0],
            dest.positionValue[1],
            dest.positionValue[2]);
    Vector3f originV(origin.positionValue[0],
            origin.positionValue[1],
            origin.positionValue[2]);
    movedVector=destV-originV;
    return 1;
}


int calculateDeltaDistanceByTeach_F(const PalletMiddleInfo &tmpMiddleInfo,int row,int layer,
                                    Vector3f &totalVectorOut)
{

    if(row>=tmpMiddleInfo.configInfo.stackStylePointInfo.size())
    {
        qDebug()<<"error,row>=tmpMiddleInfo.configInfo.stackStylePointInfo.size()";
        return 0;
    }
    if(6>tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue.size())
    {
        qDebug()<<"error,6>tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue.size()";
        return -1;
    }
    if(6>tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue.size())
    {
        qDebug()<<"error,6>tmpMiddleInfo.configInfo.stackStylePointInfo[1].stackPoint.positionValue.size()";
        return -2;
    }

    //第ｎ点-第１点
    Vector3f localTransVector,deltaLayerVector;
    Vector3f startV(tmpMiddleInfo.configInfo.stackStylePointInfo[1].stackPoint.positionValue[0],
            tmpMiddleInfo.configInfo.stackStylePointInfo[1].stackPoint.positionValue[1],
            tmpMiddleInfo.configInfo.stackStylePointInfo[1].stackPoint.positionValue[2]);
    Vector3f endRowV(tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[0],
            tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[1],
            tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[2]);
    Vector3f endLayerV(tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue[0],
            tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue[1],
            tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue[2]);
    localTransVector=endRowV-startV;


    if(1>=tmpMiddleInfo.configInfo.layer)
    {
        deltaLayerVector=endLayerV-endLayerV;
    }
    else
    {
        deltaLayerVector=(endLayerV-startV)/(tmpMiddleInfo.configInfo.layer-1);
    }

    totalVectorOut=localTransVector+deltaLayerVector*(layer-1);
    return 1;

}

int PalletCalculate::calculatePalletPointByRcl_F(PalletMiddleInfo &tmpMiddleInfo, int row, int col,
                                                 int layer)
{
    if(( row > tmpMiddleInfo.configInfo.row )||(1 > row) ||
        (col > tmpMiddleInfo.configInfo.col)||(1 > col) ||
        (layer > tmpMiddleInfo.configInfo.layer) ||(1 > layer))
    {   //行列层超出范围
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12855, robotId,row,col,layer);
        return -20;
    }
    if(6>tmpMiddleInfo.configInfo.stackPointBTM.positionValue.size())
    {
        qDebug()<<"error,6>tmpMiddleInfo.stackPointBTM.positionValue.size()";
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12853,robotId, 1);
        return 0;
    }

    if(row>=tmpMiddleInfo.configInfo.stackStylePointInfo.size())
    {
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12856,robotId, 1);
        qDebug()<<"error,row>=tmpMiddleInfo.configInfo.stackStylePointInfo.size()";
        return 0;
    }
    if(6>tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue.size())
    {
        qDebug()<<"error,6>tmpMiddleInfo.configInfo.stackStylePointInfo[0].stackPoint.positionValue.size()";
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12854,robotId, 1);
        return 0;
    }


    Vector3f tmpMove_total;
    int tmpKey=calculateDeltaDistanceByTeach_F(tmpMiddleInfo,row,layer,tmpMove_total);
    if(1!=tmpKey)
    {
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12857,robotId, 1);
        return tmpKey;
    }
    tmpMiddleInfo.btmPointPro=tmpMiddleInfo.configInfo.stackPointBTM;

    tmpMiddleInfo.btmPointPro.positionValue[0] += tmpMove_total[0];
    tmpMiddleInfo.btmPointPro.positionValue[1] += tmpMove_total[1];
    tmpMiddleInfo.btmPointPro.positionValue[2] += tmpMove_total[2];

    if(tmpMiddleInfo.configInfo.stackStylePointInfo[row].fleeStyleIndex-1
            >=tmpMiddleInfo.configInfo.stackPointR_nList.size() ||
            tmpMiddleInfo.configInfo.stackStylePointInfo[row].fleeStyleIndex<=0)
    {
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12863,robotId,
               tmpMiddleInfo.configInfo.stackStylePointInfo[row].fleeStyleIndex);
        return 0;
    }

    if(tmpMiddleInfo.configInfo.stackStylePointInfo[row].approachStyleIndex-1
            >=tmpMiddleInfo.configInfo.stackPointA_nList.size()||
            tmpMiddleInfo.configInfo.stackStylePointInfo[row].approachStyleIndex<=0)
    {
        addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12864,robotId, tmpMiddleInfo.configInfo.stackStylePointInfo[row].approachStyleIndex);
        return 0;
    }

    tmpMiddleInfo.pointAnPointPro=tmpMiddleInfo.configInfo.stackPointA_nList
            [tmpMiddleInfo.configInfo.stackStylePointInfo[row].approachStyleIndex-1].points;
    tmpMiddleInfo.pointRnPointPro=tmpMiddleInfo.configInfo.stackPointR_nList
            [tmpMiddleInfo.configInfo.stackStylePointInfo[row].fleeStyleIndex-1].points;
    for(int i=0; i<tmpMiddleInfo.configInfo.stackProximityCount; i++)
    {
        if(6>tmpMiddleInfo.pointAnPointPro[i].positionValue.size())
        {
            qDebug()<<"error,6>tmpMiddleInfo.pointAnPointPro[i].positionValue.size()"<<i;
            addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12851,robotId, i+1);
            return 0;
        }
        tmpMiddleInfo.pointAnPointPro[i].positionValue[0] += tmpMove_total[0];
        tmpMiddleInfo.pointAnPointPro[i].positionValue[1] += tmpMove_total[1];
        tmpMiddleInfo.pointAnPointPro[i].positionValue[2] += tmpMove_total[2];
        tmpMiddleInfo.pointAnPointPro[i].positionValue[3]=
                tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[3];
        tmpMiddleInfo.pointAnPointPro[i].positionValue[4]=
                tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[4];
        tmpMiddleInfo.pointAnPointPro[i].positionValue[5]=
                tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[5];
    }
    for(int i=0; i<tmpMiddleInfo.configInfo.stackFleeCount; i++)
    {
        if(6>tmpMiddleInfo.pointRnPointPro[i].positionValue.size())
        {
            qDebug()<<"error,6>tmpMiddleInfo.pointRnPointPro[i].positionValue.size()"<<i;
            addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12852,robotId, i+1);
            return 0;
        }
        tmpMiddleInfo.pointRnPointPro[i].positionValue[0] += tmpMove_total[0];
        tmpMiddleInfo.pointRnPointPro[i].positionValue[1] += tmpMove_total[1];
        tmpMiddleInfo.pointRnPointPro[i].positionValue[2] += tmpMove_total[2];
        tmpMiddleInfo.pointRnPointPro[i].positionValue[3]=
                tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[3];
        tmpMiddleInfo.pointRnPointPro[i].positionValue[4]=
                tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[4];
        tmpMiddleInfo.pointRnPointPro[i].positionValue[5]=
                tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[5];
    }

    tmpMiddleInfo.btmPointPro.positionValue[3]=
            tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[3];
    tmpMiddleInfo.btmPointPro.positionValue[4]=
            tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[4];
    tmpMiddleInfo.btmPointPro.positionValue[5]=
            tmpMiddleInfo.configInfo.stackStylePointInfo[row].stackPoint.positionValue[5];

    return 1;
}

int transPointToJson(const PointPro &pointIn, QJsonObject &obj_out)
{
    QJsonObject obj_item_point;
    obj_item_point.insert("jointFlag", pointIn.jointFlag);
    obj_item_point.insert("pointNum", pointIn.pointNum);
    obj_item_point.insert("groupNum", pointIn.groupNum);
    obj_item_point.insert("ufValue", pointIn.ufValue);
    obj_item_point.insert("utValue", pointIn.utValue);
    obj_item_point.insert("recordStatus", pointIn.recordStatus);
    obj_item_point.insert("configString", QString::fromStdString(pointIn.configString));


    QJsonArray arry_configInt,arry_positionValue,arry_extPositionValue;

    for(int i=0;i<pointIn.configInt.size();i++)
    {
        arry_configInt.append(QJsonValue(pointIn.configInt[i]));
    }
    obj_item_point.insert("configInt", arry_configInt);

    for(int i=0;i<pointIn.positionValue.size();i++)
    {
        arry_positionValue.append(QJsonValue(pointIn.positionValue[i]));
    }
    obj_item_point.insert("positionValue", arry_positionValue);

    for(int i=0;i<pointIn.extPositionValue.size();i++)
    {
        arry_extPositionValue.append(QJsonValue(pointIn.extPositionValue[i]));
    }
    obj_item_point.insert("extPositionValue", arry_extPositionValue);

    obj_out=obj_item_point;
    return 1;

}


int transCoordinateToJson(const StackInfo &stackInfoIn, QJsonObject &obj_out)
{
    QJsonObject obj_stackinfo;
    obj_stackinfo.insert("pathStyleCount", stackInfoIn.pathStyleCount);
    obj_stackinfo.insert("stackUpDown", stackInfoIn.stackUpDown);
    obj_stackinfo.insert("row", stackInfoIn.row);
    obj_stackinfo.insert("col", stackInfoIn.col);
    obj_stackinfo.insert("layer", stackInfoIn.layer);
    obj_stackinfo.insert("execIncr", stackInfoIn.execIncr);
    obj_stackinfo.insert("auxiliary", stackInfoIn.auxiliary);
    obj_stackinfo.insert("execOrder", stackInfoIn.execOrder);
    obj_stackinfo.insert("teachOrder", stackInfoIn.teachOrder);
    obj_stackinfo.insert("stackType", stackInfoIn.stackType);
    obj_stackinfo.insert("stackNumber", stackInfoIn.stackNumber);
    obj_stackinfo.insert("stackProximityCount", stackInfoIn.stackProximityCount);
    obj_stackinfo.insert("stackFleeCount", stackInfoIn.stackFleeCount);
    obj_stackinfo.insert("stackStyleCount", stackInfoIn.stackStyleCount);
    obj_stackinfo.insert("stackRegisterNumber", stackInfoIn.stackRegisterNumber);
    obj_stackinfo.insert("stackPointCount", stackInfoIn.stackPointCount);
    obj_stackinfo.insert("stackFinishOutputMode", stackInfoIn.stackFinishOutputMode);
    obj_stackinfo.insert("stackFinishOutputDoNum", stackInfoIn.stackFinishOutputDoNum);
//    QByteArray tmpName=QString::fromStdString(stackInfoIn.stackName).toUtf8();
    obj_stackinfo.insert("stackName", QString::fromStdString(stackInfoIn.stackName));

    //stackPointBTM
    QJsonObject obj_stackPointBTM;
    transPointToJson(stackInfoIn.stackPointBTM,obj_stackPointBTM);
    obj_stackinfo.insert("stackPointBTM", obj_stackPointBTM);

    qDebug()<<"stackPointA_nList"<<stackInfoIn.stackPointA_nList.size()<<"stackPointR_nList"<<stackInfoIn.stackPointR_nList.size();
    //stackPointA_nList
    QJsonArray  obj_stackPointA_nList;
    QJsonObject obj_point;
    for(int i=0;i<stackInfoIn.stackPointA_nList.size();i++)
    {
        //list
        QJsonArray  obj_point_list;
        for(int j=0;j<stackInfoIn.stackPointA_nList[i].points.size();j++)
        {
            transPointToJson(stackInfoIn.stackPointA_nList[i].points[j],obj_point);
            obj_point_list.append(QJsonValue(obj_point));
        }
        obj_stackPointA_nList.append(obj_point_list);
    }
    obj_stackinfo.insert("stackPointA_nList", obj_stackPointA_nList);

    //stackPointR_n
    QJsonArray  obj_stackPointR_nList;
    QJsonObject obj_pointR;
    for(int i=0;i<stackInfoIn.stackPointR_nList.size();i++)
    {
        //list
        QJsonArray  obj_point_listR;
        for(int j=0;j<stackInfoIn.stackPointR_nList[i].points.size();j++)
        {
            transPointToJson(stackInfoIn.stackPointR_nList[i].points[j],obj_pointR);
            obj_point_listR.append(obj_pointR);
        }
        obj_stackPointR_nList.append(obj_point_listR);
    }
    obj_stackinfo.insert("stackPointR_nList", obj_stackPointR_nList);


    QJsonArray  obj_xyzwpr_list;
    for(int i=0;i<stackInfoIn.intervalEnable.size();i++)
    {
        obj_xyzwpr_list.append(QJsonValue(stackInfoIn.intervalEnable[i]));
    }
    obj_stackinfo.insert("intervalEnable", obj_xyzwpr_list);

    QJsonArray  obj_intervalValue_list;
    for(int i=0;i<stackInfoIn.intervalValue.size();i++)
    {
        obj_intervalValue_list.append(QJsonValue(stackInfoIn.intervalValue[i]));
    }
    obj_stackinfo.insert("intervalValue", obj_intervalValue_list);

    //stackStylePointInfo
    QJsonArray  obj_stackStylePointInfo_listR;

    for(int i=0;i<stackInfoIn.stackStylePointInfo.size();i++)
    {
        QJsonObject obj_stackStylePointInfo;
        QJsonObject obj_tmpPoint;
        //1point
        transPointToJson(stackInfoIn.stackStylePointInfo[i].stackPoint,obj_tmpPoint);
        obj_stackStylePointInfo.insert("stackPoint", obj_tmpPoint);

        //list
         QJsonArray  obj_tmpList;
         for(int j=0;j<stackInfoIn.stackStylePointInfo[i].rowColumnLayer.size();j++)
         {
             obj_tmpList.append(QJsonValue(stackInfoIn.stackStylePointInfo[i].rowColumnLayer[j]));
         }
         obj_stackStylePointInfo.insert("rowColumnLayer", obj_tmpList);

         //
         obj_stackStylePointInfo.insert("approachStyleIndex", stackInfoIn.stackStylePointInfo[i].approachStyleIndex);
         obj_stackStylePointInfo.insert("fleeStyleIndex", stackInfoIn.stackStylePointInfo[i].fleeStyleIndex);

        obj_stackStylePointInfo_listR.append(QJsonValue(obj_stackStylePointInfo));
    }
    obj_stackinfo.insert("stackStylePointInfo", obj_stackStylePointInfo_listR);


    obj_out=obj_stackinfo;
    return 1;
}


int PalletCalculate::writePalletInfo(QString filePathIn)
{
    qDebug() << "===writePalletInfo fileNameIn===="<<filePathIn<<endl;

    QFile file(filePathIn);
    if(!file.open(QIODevice::WriteOnly| QIODevice::Text))
    {
        //消息预留
        //保存R寄存器数据到文件失败
        return -1;
    }
    else
    {

        QJsonObject obj_root;
        QJsonObject obj_level1;
        QJsonArray arry_itemList;
        int palletSize=palletMiddleInfoList.size();
        obj_level1.insert("size", palletSize);

        QString tmpNodeName;
        for(int i=0;i<palletMiddleInfoList.size();i++)
        {
            QJsonObject obj_item;
            tmpNodeName="Pallet"+QString::number(i+1);
            QJsonObject obj_item_point;
            transCoordinateToJson(palletMiddleInfoList[i].configInfo,obj_item_point);
            obj_item.insert(tmpNodeName, obj_item_point);
            arry_itemList.append(obj_item);
            qDebug()<<"pallet"<<i;
        }

        obj_level1.insert("RValue_list", arry_itemList);
        obj_root.insert("regR_info", obj_level1);

        QJsonDocument jsonDoc(obj_root);
        QString tmpString=jsonDoc.toJson();

        //写入内容,这里需要转码，否则报错。
    //    QByteArray str = tmpString.toUtf8();
         QByteArray str = tmpString.toLatin1();
        //写入QByteArray格式字符串
        file.write(str);

        file.close();
        file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
    }


    return 1;
}


int getPointProFromJson(const QJsonObject &obj_item_point, PointPro &pointOut)
{
    PointPro tmpPoint;
    if(false==obj_item_point.contains("jointFlag"))
    {

        return -1;
    }
    tmpPoint.jointFlag=obj_item_point.value("jointFlag").toInt();
    if(false==obj_item_point.contains("pointNum"))
    {

        return -2;
    }
    tmpPoint.pointNum=obj_item_point.value("pointNum").toInt();
    if(false==obj_item_point.contains("groupNum"))
    {

        return -3;
    }
    tmpPoint.groupNum=obj_item_point.value("groupNum").toInt();
    if(false==obj_item_point.contains("ufValue"))
    {

        return -4;
    }
    tmpPoint.ufValue=obj_item_point.value("ufValue").toInt();
    if(false==obj_item_point.contains("utValue"))
    {

        return -5;
    }
    tmpPoint.utValue=obj_item_point.value("utValue").toInt();
    if(false==obj_item_point.contains("recordStatus"))
    {

        return -6;
    }
    tmpPoint.recordStatus=obj_item_point.value("recordStatus").toInt();
    if(false==obj_item_point.contains("configString"))
    {

        return -7;
    }
    tmpPoint.configString=obj_item_point.value("configString").toString().toStdString();
    //
    if(false==obj_item_point.contains("configInt"))
    {

        return -8;
    }
    QJsonArray tmp_arry=obj_item_point.value("configInt").toArray();
    for(int i=0;i<tmp_arry.size();i++)
    {
        tmpPoint.configInt.push_back(tmp_arry[i].toInt());
    }
    //
    if(false==obj_item_point.contains("positionValue"))
    {

        return -9;
    }
    tmp_arry=obj_item_point.value("positionValue").toArray();
    for(int i=0;i<tmp_arry.size();i++)
    {
        tmpPoint.positionValue.push_back(tmp_arry[i].toDouble());
    }

    //
    if(false==obj_item_point.contains("extPositionValue"))
    {

        return -10;
    }
    tmp_arry=obj_item_point.value("extPositionValue").toArray();
    for(int i=0;i<tmp_arry.size();i++)
    {
        tmpPoint.extPositionValue.push_back(tmp_arry[i].toDouble());
    }
    pointOut=tmpPoint;
    return 1;

}


int getCoordinateFromJson(const QJsonObject &obj_coordinate,
                                             StackInfo &coordinateOut,QString &commentOut)
{
    StackInfo tmpStack;

    if(false==obj_coordinate.contains("pathStyleCount"))
    {
        commentOut+=" pathStyleCount";
        return -1;
    }
    tmpStack.pathStyleCount=obj_coordinate.value("pathStyleCount").toInt();

    if(false==obj_coordinate.contains("stackNumber"))
    {
        commentOut+=" stackNumber";
        return -1;
    }
    tmpStack.stackNumber=obj_coordinate.value("stackNumber").toInt();

    if(false==obj_coordinate.contains("stackName"))
    {
        commentOut+=" stackName";
        return -2;
    }
    tmpStack.stackName=obj_coordinate.value("stackName").toString().toStdString();

    if(false==obj_coordinate.contains("stackUpDown"))
    {
        commentOut+=" stackUpDown";
        return -3;
    }
    tmpStack.stackUpDown=obj_coordinate.value("stackUpDown").toInt();

    if(false==obj_coordinate.contains("row"))
    {
        commentOut+=" row";
        return -3;
    }
    tmpStack.row=obj_coordinate.value("row").toInt();

    if(false==obj_coordinate.contains("col"))
    {
        commentOut+=" col";
        return -3;
    }
    tmpStack.col=obj_coordinate.value("col").toInt();

    if(false==obj_coordinate.contains("layer"))
    {
        commentOut+=" layer";
        return -3;
    }
    tmpStack.layer=obj_coordinate.value("layer").toInt();

    if(false==obj_coordinate.contains("execIncr"))
    {
        commentOut+=" execIncr";
        return -3;
    }
    tmpStack.execIncr=obj_coordinate.value("execIncr").toInt();

    if(false==obj_coordinate.contains("auxiliary"))
    {
        commentOut+=" auxiliary";
        return -3;
    }
    tmpStack.auxiliary=obj_coordinate.value("auxiliary").toInt();

    if(false==obj_coordinate.contains("execOrder"))
    {
        commentOut+=" execOrder";
        return -3;
    }
    tmpStack.execOrder=obj_coordinate.value("execOrder").toInt();

    if(false==obj_coordinate.contains("teachOrder"))
    {
        commentOut+=" teachOrder";
        return -3;
    }
    tmpStack.teachOrder=obj_coordinate.value("teachOrder").toInt();

    if(false==obj_coordinate.contains("stackType"))
    {
        commentOut+=" stackType";
        return -3;
    }
    tmpStack.stackType=obj_coordinate.value("stackType").toInt();

    if(false==obj_coordinate.contains("stackProximityCount"))
    {
        commentOut+=" stackProximityCount";
        return -3;
    }
    tmpStack.stackProximityCount=obj_coordinate.value("stackProximityCount").toInt();

    if(false==obj_coordinate.contains("stackFleeCount"))
    {
        commentOut+=" stackFleeCount";
        return -3;
    }
    tmpStack.stackFleeCount=obj_coordinate.value("stackFleeCount").toInt();

    if(false==obj_coordinate.contains("stackStyleCount"))
    {
        commentOut+=" stackStyleCount";
        return -3;
    }
    tmpStack.stackStyleCount=obj_coordinate.value("stackStyleCount").toInt();

    if(false==obj_coordinate.contains("stackRegisterNumber"))
    {
        commentOut+=" stackRegisterNumber";
        return -3;
    }
    tmpStack.stackRegisterNumber=obj_coordinate.value("stackRegisterNumber").toInt();

    if(false==obj_coordinate.contains("stackPointCount"))
    {
        commentOut+=" stackPointCount";
        return -3;
    }
    tmpStack.stackPointCount=obj_coordinate.value("stackPointCount").toInt();

    if(false==obj_coordinate.contains("stackFinishOutputMode"))
    {
        commentOut+=" stackFinishOutputMode";
        return -3;
    }
    tmpStack.stackFinishOutputMode=obj_coordinate.value("stackFinishOutputMode").toInt();

    if(false==obj_coordinate.contains("stackFinishOutputDoNum"))
    {
        commentOut+=" stackFinishOutputDoNum";
        return -3;
    }
    tmpStack.stackFinishOutputDoNum=obj_coordinate.value("stackFinishOutputDoNum").toInt();

    if(false==obj_coordinate.contains("intervalEnable"))
    {
        commentOut+=" intervalEnable";
        return -4;
    }
    QJsonArray obj_xyzwpr=obj_coordinate.value("intervalEnable").toArray();
    for(int i=0;i<obj_xyzwpr.size();i++)
    {
        tmpStack.intervalEnable.push_back(obj_xyzwpr[i].toInt());

    }

    if(false==obj_coordinate.contains("intervalValue"))
    {
        commentOut+=" intervalValue";
        return -4;
    }
    QJsonArray obj_intervalValue=obj_coordinate.value("intervalValue").toArray();
    for(int i=0;i<obj_intervalValue.size();i++)
    {
        tmpStack.intervalValue.push_back(obj_intervalValue[i].toDouble());

    }

    if(false==obj_coordinate.contains("stackPointBTM"))
    {
        commentOut+=" stackPointBTM";
        return -4;
    }
    QJsonObject obj_stackPointBTM=obj_coordinate.value("stackPointBTM").toObject();
    getPointProFromJson(obj_stackPointBTM,tmpStack.stackPointBTM);


    if(false==obj_coordinate.contains("stackPointA_nList"))
    {
        commentOut+=" stackPointA_nList";
        return -4;
    }
    QJsonArray obj_stackPointA_nList=obj_coordinate.value("stackPointA_nList").toArray();
    for(int i=0;i<obj_stackPointA_nList.size();i++)
    {
        QJsonArray obj_stackPointA_n;
        obj_stackPointA_n=obj_stackPointA_nList[i].toArray();
        //
        QJsonObject obj_point;
        PointPro tmpPoint;
        PalletPathInfo tmpStackPoint;
        for(int j=0;j<obj_stackPointA_n.size();j++)
        {
            obj_point=obj_stackPointA_n[j].toObject();
            getPointProFromJson(obj_point,tmpPoint);
            tmpStackPoint.points.push_back(tmpPoint);
        }
        tmpStack.stackPointA_nList.push_back(tmpStackPoint);
    }

    if(false==obj_coordinate.contains("stackPointR_nList"))
    {
        commentOut+=" stackPointR_nList";
        return -4;
    }
    QJsonArray obj_stackPointR_nList=obj_coordinate.value("stackPointR_nList").toArray();
    for(int i=0;i<obj_stackPointR_nList.size();i++)
    {
        QJsonArray obj_stackPointR_n;
        obj_stackPointR_n=obj_stackPointR_nList[i].toArray();
        //
        QJsonObject obj_point;
        PointPro tmpPoint;
        PalletPathInfo tmpStackPoint;
        for(int j=0;j<obj_stackPointR_n.size();j++)
        {
            obj_point=obj_stackPointR_n[j].toObject();
            getPointProFromJson(obj_point,tmpPoint);
            tmpStackPoint.points.push_back(tmpPoint);
        }
        tmpStack.stackPointR_nList.push_back(tmpStackPoint);
    }


    if(false==obj_coordinate.contains("stackStylePointInfo"))
    {
        commentOut+=" stackStylePointInfo";
        return -4;
    }
    QJsonArray obj_stackStylePointInfo=obj_coordinate.value("stackStylePointInfo").toArray();
    QJsonObject obj_sp;
    StackStylePointInfo tmpStylePointInfo;
    for(int i=0;i<obj_stackStylePointInfo.size();i++)
    {
        obj_sp=obj_stackStylePointInfo[i].toObject();

        //1p
        if(false==obj_sp.contains("stackPoint"))
        {
            commentOut+=" stackPoint";
            return -4;
        }
        QJsonObject obj_tmpstackp=obj_sp.value("stackPoint").toObject();
        getPointProFromJson(obj_tmpstackp,tmpStylePointInfo.stackPoint);

        //1list
        if(false==obj_sp.contains("rowColumnLayer"))
        {
            commentOut+=" rowColumnLayer";
            return -4;
        }
        QJsonArray obj_tmprcl=obj_sp.value("rowColumnLayer").toArray();
        for(int j=0;j<obj_tmprcl.size();j++)
        {
            tmpStylePointInfo.rowColumnLayer.push_back(obj_tmprcl[j].toInt());

        }

        //
        if(false==obj_sp.contains("approachStyleIndex"))
        {
            commentOut+=" approachStyleIndex";
            return -3;
        }
        tmpStylePointInfo.approachStyleIndex=obj_sp.value("approachStyleIndex").toInt();

        if(false==obj_sp.contains("fleeStyleIndex"))
        {
            commentOut+=" fleeStyleIndex";
            return -3;
        }
        tmpStylePointInfo.fleeStyleIndex=obj_sp.value("fleeStyleIndex").toInt();

        tmpStack.stackStylePointInfo.push_back(tmpStylePointInfo);

    }

    coordinateOut=tmpStack;
    return 1;
}


int PalletCalculate::readConfigFile(QString filePathIn, QString &readComent)
{
    qDebug()<<"readConfigFile::readConfigFile"<<filePathIn;
    palletMiddleInfoList.clear();

    int returnFlag=-1;
    QFile file(filePathIn);
    if(file.exists())
    {
        if(file.open(QIODevice::ReadOnly))
        {

            QByteArray receiveByteArry = file.read(file.bytesAvailable());
            QJsonParseError jsonError;
            QJsonDocument receiveJsonDoc = QJsonDocument::fromJson(receiveByteArry, &jsonError);
            if(jsonError.error != QJsonParseError::NoError )
            {
              qDebug() << "readCoordinateFile_json JsonFile error "<<jsonError.error;

              return -2;
            }
            QJsonObject redeiveJsonObj = receiveJsonDoc.object();

            if(false==redeiveJsonObj.contains("regR_info"))
            {

                return -3;
            }
            QJsonObject obj_level1=redeiveJsonObj.value("regR_info").toObject();
            if(false==obj_level1.contains("size"))
            {

                return -4;
            }
            int size=obj_level1.value("size").toInt();
            if(false==obj_level1.contains("RValue_list"))
            {

                return -5;
            }
            QJsonArray arry_itemList=obj_level1.value("RValue_list").toArray();

            if(arry_itemList.size()!=size)
            {

                return -6;
            }
            QString tmpNodeName;
            for(int k=0;k<size;k++)
            {
                PalletMiddleInfo tmpMiddleInfo;
                QJsonObject obj_item=arry_itemList[k].toObject();
                tmpNodeName="Pallet"+QString::number(k+1);
                if(false==obj_item.contains(tmpNodeName))
                {

                    return -100-k;
                }
                QJsonObject obj_item_point=obj_item[tmpNodeName].toObject();

                if(1!=getCoordinateFromJson(obj_item_point,tmpMiddleInfo.configInfo,readComent))
                {

                    return -1000-k;
                }

                tmpMiddleInfo.configInfo.stackNumber=k+1;
                palletMiddleInfoList.push_back(tmpMiddleInfo);
            }


            file.close();
            file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
        }
        returnFlag= 1;
    }
    else
    {
//        regPrValue.resize(REG_SIZE);
        returnFlag= -1;
    }



    return returnFlag;
}




int  PalletCalculate::calculateNextRcl(int palletIndex, int &row, int &col, int &layer)
{
    if( palletIndex >= palletMiddleInfoList.size() || 0 > palletIndex)
    {//超出码垛范围
        //addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 4004, 1);
        return -1;
    }
    PalletMiddleInfo tmpMiddleInfo=palletMiddleInfoList[palletIndex];

    if(tmpMiddleInfo.configInfo.stackUpDown == ENUM_STACK_UP)//码垛
    {
        if(tmpMiddleInfo.configInfo.execOrder == ENUM_STACK_ORDER_RCL)//执行顺序是行列层
        {
            if(1==tmpMiddleInfo.configInfo.execIncr)//增加
            {
                if(row < tmpMiddleInfo.configInfo.row)//行没结束
                {
                    row++;//下一行
                }
                else if(col < tmpMiddleInfo.configInfo.col)//列没结束
                {
                    row = 1;
                    col++;
                }
                else if(layer < tmpMiddleInfo.configInfo.layer)//层没结束
                {
                    row = 1;
                    col = 1;
                    layer++;
                }
                else//码垛完成
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;  //码垛完成信号
                  //  std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return -1;
                }
                if (row == tmpMiddleInfo.configInfo.row && col == tmpMiddleInfo.configInfo.col && layer == tmpMiddleInfo.configInfo.layer)
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
              //      std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return 1;
                }

            }
            else if(-1== tmpMiddleInfo.configInfo.execIncr) //减少
            {
                if(row > 1)//行没结束
                {
                    row--;
                }
                else if(col > 1)//列没结束
                {
                    row = tmpMiddleInfo.configInfo.row ;
                    col--;
                }
                else if(layer < tmpMiddleInfo.configInfo.layer)//层没结束
                {
                    row = tmpMiddleInfo.configInfo.row;
                    col = tmpMiddleInfo.configInfo.col;
                    layer ++;
                }
                else //最后一层，码垛完成
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
                    return -1;
                }
                //2017-4-14
                if (row == 1 && col == 1 && layer == tmpMiddleInfo.configInfo.layer)
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
               //     std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return 1;
                }
            }//end else//减少
            else
            {

            }
        }
        else if(tmpMiddleInfo.configInfo.execOrder == ENUM_STACK_ORDER_CRL) //列行层
        {
            if(1==tmpMiddleInfo.configInfo.execIncr)//增加
            {
                if(col < tmpMiddleInfo.configInfo.col)//列没结束
                {
                    col++;
                }
                else if(row < tmpMiddleInfo.configInfo.row)//行没结束
                {
                    row++;
                    col = 1;
                }
                else if(layer < tmpMiddleInfo.configInfo.layer)//层没结束
                {
                    row = 1;
                    col = 1;
                    layer++;
                }
                else//最后一层，码垛完成
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
                    return -1;
                }

                //2017-4-14
                if (row == tmpMiddleInfo.configInfo.row && col < tmpMiddleInfo.configInfo.col && layer == tmpMiddleInfo.configInfo.layer)
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
                    return 1;
                }
            }
            else if(-1== tmpMiddleInfo.configInfo.execIncr) //减少
            {
                if(col > 1)//列没结束
                {
                    col--;
                }
                else if(row > 1)//行没结束
                {
                    row--;
                    col = tmpMiddleInfo.configInfo.col;
                }
                else if(layer < tmpMiddleInfo.configInfo.layer)//层没结束
                {
                    row = tmpMiddleInfo.configInfo.row;
                    col = tmpMiddleInfo.configInfo.col;
                    layer++;
                }
                else//最后一层，码垛完成
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
                    return -1;
                }
                //2017-4-14
                if (row == 1 && col == 1 && layer == tmpMiddleInfo.configInfo.layer)
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
              //      std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return 1;
                }
            }//end else//减少
            else
            {

            }
        }//end else//列行层
    }
    else if (tmpMiddleInfo.configInfo.stackUpDown == ENUM_STACK_DOWN) //拆垛
    {
        if(tmpMiddleInfo.configInfo.execOrder == ENUM_STACK_ORDER_RCL)//执行顺序是行列层
        {
            if(1==tmpMiddleInfo.configInfo.execIncr)//增加
            {
                if(row > 1)//行没结束
                {
                    row--;
                }
                else if(col > 1)//列没结束
                {
                    row = tmpMiddleInfo.configInfo.row;
                    col--;
                }
                else if(layer > 1)//层没结束
                {
                    row = tmpMiddleInfo.configInfo.row;
                    col = tmpMiddleInfo.configInfo.col;
                    layer--;
                }
                else//第一层，拆垛完成
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
                    return 0;
                }
                //2017-4-14
                if (row == 1 && col == 1 && layer == 1)
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
            //        std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return 1;
                }
            }
            else if(-1 == tmpMiddleInfo.configInfo.execIncr) //减少
            {
                if(row < tmpMiddleInfo.configInfo.row)//行没结束
                {
                    row++;
                }
                else if(col < tmpMiddleInfo.configInfo.col)//列没结束
                {
                    row = 1;
                    col++;
                }
                else if(layer > 1)//层没结束
                {
                    row = 1;
                    col = 1;
                    layer--;
                }
                else //第一层，拆垛完成
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
                    return 0;
                }
                //2017-4-14
                if (row == tmpMiddleInfo.configInfo.row && col == tmpMiddleInfo.configInfo.col && layer == 1)
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
        //            std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return 1;
                }
            }//end else//减少
        }
        else if(tmpMiddleInfo.configInfo.execOrder == ENUM_STACK_ORDER_CRL) //列行层
        {
            if(1==tmpMiddleInfo.configInfo.execIncr)//增加
            {
                if(col > 1 )//列没结束
                {
                    col--;
                }
                else if(row > 1)//行没结束
                {
                    row--;
                    col = tmpMiddleInfo.configInfo.col;
                }
                else if(layer > 1)//层没结束
                {
                    row = tmpMiddleInfo.configInfo.row;
                    col = tmpMiddleInfo.configInfo.col;
                    layer--;
                }
                else//最后一层，码垛完成
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
                    return 0;
                }
                //2017-4-14
                if (row == 1 && col == 1 && layer == 1)
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
       //             std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return 1;
                }
            }
            else if(-1==tmpMiddleInfo.configInfo.execIncr)//减少
            {
                if(col < tmpMiddleInfo.configInfo.col)//列没结束
                {
                    col++;
                }
                else if(row < tmpMiddleInfo.configInfo.row)//行没结束
                {
                    row++;
                    col = 1;
                }
                else if(layer > 1)//层没结束
                {
                    row = 1;
                    col = 1;
                    layer--;
                }
                else //最后一层，拆垛完成
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
                    return 0;
                }
                //2017-4-14
                if (row == tmpMiddleInfo.configInfo.row && col == tmpMiddleInfo.configInfo.col && layer == 1)
                {
                    tmpMiddleInfo.finishOutput = ENUM_STACK_WORK_FINISH;
         //           std::cout << "-----------finishOutput-------------" << palletVector[palletIndex].finishOutput << "/n/n/n/n" ;
                    return 1;
                }
            }//end else//减少
        }//end else//列行层
    }//end else//拆垛
    return 1;
}


int PalletCalculate::getPalletPointData(struct PalletPoint palletPoint, struct PointPro &p)
{

    switch (palletPoint.pointType)
    {
        case ENUM_STACK_POINT_BTM://堆叠点
        {
             p = palletMiddleInfoList[palletPoint.palletNumber].btmPointPro;

            return 1;
        }
        case ENUM_STACK_POINT_AN://接近点
        {
            if((palletPoint.pointNumber < 0)
                    ||palletPoint.pointNumber >= palletMiddleInfoList[palletPoint.palletNumber].pointAnPointPro.size())
            {
                addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12860, robotId,
                       palletPoint.pointNumber+1);
                return -2;
            }
            p = palletMiddleInfoList[palletPoint.palletNumber].pointAnPointPro[palletPoint.pointNumber];


            return 1;
        }
        case ENUM_STACK_POINT_RN://逃离点
        {
            if((palletPoint.pointNumber < 0)
                    ||palletPoint.pointNumber >= palletMiddleInfoList[palletPoint.palletNumber].pointRnPointPro.size())
            {
                addMsg(ENUM_MSG_ERROR, "PalletCalculate", "PalletCalculate", 12861, robotId,
                       palletPoint.pointNumber+1);
                return -3;//接近点号超过范围
            }
            p = palletMiddleInfoList[palletPoint.palletNumber].pointRnPointPro[palletPoint.pointNumber];

            return 1;
        }
        default:
        {
            return -4;//不支持点类型
        }
    }
    return 0;
}

void PalletCalculate::addMsg(int messageLevel, string componentName, string messageType,
                             int messageCode, int robotId,
                                     int parameter1, int parameter2, int parameter3, int parameter4, QString message)
{

    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {

    case 12864:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺接近路径样式%1号未示教！").arg(parameter1)
                .arg(parameter2).arg(parameter3)+message;
    }
        break;
    case 12863:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺逃离路径样式%1号未示教！").arg(parameter1)
                .arg(parameter2).arg(parameter3)+message;
    }
        break;
    case 12862:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺号%1超范围！").arg(parameter1)
                .arg(parameter2).arg(parameter3)+message;
    }
        break;
    case 12861:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺逃离点%1超范围！").arg(parameter1)
                .arg(parameter2).arg(parameter3)+message;
    }
        break;
    case 12860:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺接近点%1超范围！").arg(parameter1)
                .arg(parameter2).arg(parameter3)+message;
    }
        break;
    case 12858:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺已经完成，行%1列%2层%3,没有下一个点！").arg(parameter1)
                .arg(parameter2).arg(parameter3)+message;
    }
        break;
    case 12857:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺点未示教完整！").arg(parameter1)
                .arg(parameter2).arg(parameter3)+message;
    }
        break;
    case 12856:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",样式点数量不足！").arg(parameter1)
                .arg(parameter2).arg(parameter3)+message;
    }
        break;
    case 12855:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺行%1列%2层%3超范围！").arg(parameter1)
                .arg(parameter2).arg(parameter3)+message;
    }
        break;
    case 12854:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺顶层点%1没有示教位置！").arg(parameter1)
                +message;
    }
        break;
    case 12853:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺堆跺点%1没有示教位置！").arg(parameter1)
                +message;
    }
        break;
    case 12852:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺逃离点%1没有示教位置！").arg(parameter1)
                +message;
    }
        break;
    case 12851:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺接近点%1没有示教位置！").arg(parameter1)
                +message;
    }
        break;
    case 12850:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",码跺参数文件palletConfig.json读取失败,")+message;
    }
        break;



    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);


}

